/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates.subsystems;

import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.networktables.NetworkTable;
import edu.wpi.first.wpilibj.networktables.NetworkTableKeyNotDefined;
import edu.wpi.first.wpilibj.tables.TableKeyNotDefinedException;
        


/**
 *
 * @author Jarrod Risley
 */
public class KittyNet extends Subsystem {
    
    /*
     * Global Class Variables
     */
    private NetworkTable kittyNet;
    private String subsystemName;
    
    private double horizontalDistance = 0;
    private double hypotenuse = 0;
    private double recenteringDistance = 0;
    private double xError;
    private double yError;
    public boolean gotData = false;
    
    // Put methods for controlling this subsystem
    // here. Call these from Commands.
    protected void initDefaultCommand() {
    } // end initDefaultCommand
    
    
    // CONSTRUCTOR //
    
    
    public KittyNet(String name) {
        
        System.out.println("Creating KittyNet " + name + "...");
        subsystemName = name;
        
        kittyNet = NetworkTable.getTable("10.39.58.2");
        
        
    } // end VisualTrackingSubsystem Constructor
    
    
    // METHODS //
    
    
    /**
     * Grabs the vision information from KittyNet.
     */
    
    public void grabData() {
        
        
        gotData = false;
        
        if (kittyNet.isConnected()) { 
            
            
            try {
                
//                hypotenuse = kittyNet.getNumber("hypotenuse");
//                horizontalDistance = kittyNet.getNumber("horizontalDistance");
//                recenteringDistance = kittyNet.getNumber("recenteringDistance");    
//                
//                System.out.println("HorizontalDistance: " + horizontalDistance);
//                System.out.println("Hypotenuse: " + hypotenuse);
//                System.out.println("Recentering Distance: " + recenteringDistance);
//                
//                System.out.println("hypotenuse: " + kittyNet.getNumber("hypotenuse"));
//
//                System.out.println(kittyNet.getNumber("horizontalDistance"));
//                
                xError = kittyNet.getNumber("xError");
                yError = kittyNet.getNumber("yError");
                
            } catch (TableKeyNotDefinedException e) {
                System.out.println(e.getMessage());
            } // end try-catch
            
            
            
            gotData = true;
            
        } else {
            System.out.println("Data has not been aquired. Please contact "
                    + "network administrator, also known as Jarrod Risley, "
                    + "should he have any idea what is happening.");
        } // end if-else
        
    } // end grabData
    
    /**
     * Gets the error on the X axis.
     * @return How far the center of the blob is from the center of the screen.
     */
    
    public double getXError() {
        return kittyNet.getNumber("xError");
    } // end getXError
    
    /**
     * Gets the error on the Y axis.
     * @return How far the center of the blob is from the center of the screen.
     */
    public double getYError() {
        return kittyNet.getNumber("yError");
    } // end getYError
    
    /**
     * Checks to see if the robot is connected to the NetworkTable.
     * @return 
     */
    public boolean isConnected() {
        return kittyNet.isConnected();
    } // end isConnected
    
} // end KittyNet
